#include <ESP8266WiFi.h>
#include <WiFiUdp.h>
#include<Servo.h>                  //加载舵机库
Servo myservo;                     //定义一个舵机对象
Servo myservo2; 
int _servo = 14;                   //指定IO14(D5)引脚
int _servo2 = 2;  
const char* ssid = "esp8266";
const char* password = "12345678";

WiFiUDP Udp;
unsigned int localUdpPort = 4210;  // local port to listen on
char incomingPacket[255];  // buffer for incoming packets
char  replyPacket[] = "Hi there! Got the message :-)";  // a reply string to send back

void setAngle(String context,int angle, int fast){
      if (context == "up")
        for(int i=angle;i>=0;i-fast)             //控制舵机角0~90°
        {
          Serial.println("Up");
          myservo2.write(i);                
          delay(20);
        }
      if (context == "down")
        for(int i=0;i<angle;i+fast)             //控制舵机角0~90°
      {
        myservo2.write(i);                
        delay(20);
      }
      if (context == "left")
        for(int i=0;i<angle;i+fast)             //控制舵机角0~90°
      {
        myservo.write(i);                
        delay(20);
      }      
      if (context == "right")
        for(int i=angle; i>=0;i-fast)             //控制舵机角0~90°
      {
        myservo.write(i);                
        delay(20);
      }
        
}
void setup()
{
  myservo.attach(_servo, 540,2400);          //设置指定的IO为舵机
  myservo2.attach(_servo2,540,2400);  
  Serial.begin(115200);
  Serial.println();

  Serial.printf("Connecting to %s ", ssid);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED)
  {
    delay(500);
    Serial.print(".");
  }
  Serial.println(" connected");

  Udp.begin(localUdpPort);
  Serial.printf("Now listening at IP %s, UDP port %d\n", WiFi.localIP().toString().c_str(), localUdpPort);
}


void loop()
{
  int packetSize = Udp.parsePacket();
  if (packetSize)
  {
    // receive incoming UDP packets
    Serial.printf("Received %d bytes from %s, port %d\n", packetSize, Udp.remoteIP().toString().c_str(), Udp.remotePort());
    int len = Udp.read(incomingPacket, 255);
    // Serial.printf("%s",len);
    if (len > 0)
    {
      incomingPacket[len] = 0;
    }
    Serial.printf("Check Result\n");
    String context;
    context = incomingPacket;
    int fast = 10;
    int angle = 180; 
    setAngle(context,angle,fast);
    strcpy(incomingPacket,"none");

    // Serial.printf("UDP packet contents: %s\n", incomingPacket);
  
    // send back a reply, to the IP address and port we got the packet from
    Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
    Udp.write(replyPacket);
    Udp.endPacket();
  }
}